/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package teamBlitz;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.networktables.NetworkListener;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
import java.util.Vector;
import java.lang.Double;

/**
 *
 * @author danielle
 */
public class Target {

    private static PowerVector mPowerTrys;
    private Vector mDirectionTrys;
    NetworkTable targetTable;

    public Target() {
//        this.mDirectionTrys;
        this.targetTable = NetworkTable.getTable("targetTable");
        NetworkListener listner = new NetworkListener() {

            public void valueChanged(String key, Object value) {
                    double OverFar;
                    double OverNear;
                    double Score;
                    double UnderFar;
                    double UnderNear;

                    NetworkTable tmp = (NetworkTable) value;
                    System.out.println("targetTable change=" + key);

                    try {
                        OverFar = tmp.getDouble("OverFar");
                    } catch (edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined e) {
                        OverFar = 0;
                    }
                    try {
                        OverNear = tmp.getDouble("OverNear");
                    } catch (edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined e) {
                        OverNear = 0;
                    }
                    try {
                        Score = tmp.getDouble("Score");
                    } catch (edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined e) {
                        Score = 0;
                    }
                    try {
                        UnderFar = tmp.getDouble("UnderFar");
                    } catch (edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined e) {
                        UnderFar = 0;
                    }
                    try {
                        UnderNear = tmp.getDouble("UnderNear");
                    } catch (edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined e) {
                        UnderNear = 0;
                    }



                    power p = new power(Double.parseDouble(key), OverFar, OverNear,
                            Score, UnderFar, UnderNear, tmp);

                    mPowerTrys.addElement(p);
                    System.out.println("targetTable change=" + key);
            }

            public void valueConfirmed(String key, Object value) {
                System.out.println("targetTable commit=" + key);
            }
        };
        targetTable.addListenerToAll(listner);
        try {
            mPowerTrys = new PowerVector(targetTable.getSubTable("power"));
        } catch (NetworkTableKeyNotDefined ex) {
            this.mPowerTrys = new PowerVector();
//            ex.printStackTrace();
        }

    }

    private class direction {

        private class cTrys {

            double powerRPM;
            double angleDegree;
        }
        private static final int kScore = 0;
        private static final int kRightFar = 5;
        private static final int kRightNear = 6;
        private static final int kLeftFar = 7;
        private static final int kLeftNear = 8;
        double heading;
        double lastChange;
    }

    private class power {

        private static final int kScore = 0;
        private static final int kOverFar = 1;
        private static final int kOverNear = 2;
        private static final int kUnderFar = 3;
        private static final int kUnderNear = 4;
        double distanceCM;
        private double[] mAttemps;
        NetworkTable tableRef;

        private power(double dist, double OverFar, double OverNear, double score, double UnderFar, double UnderNear, NetworkTable ref) {
            distanceCM = dist;
            mAttemps = new double[5];
            mAttemps[kOverFar] = OverFar;
            mAttemps[kOverNear] = OverNear;
            mAttemps[kScore] = score;
            mAttemps[kUnderFar] = UnderFar;
            mAttemps[kUnderNear] = UnderNear;
            tableRef = ref;
        }

        private void addAttemp(double powerUsed, double angle, int result) {
            switch (result) {
                case kScore:
                    if (mAttemps[kScore] != powerUsed) {
                        if (mAttemps[kScore] == 0) {
                            mAttemps[kScore] = powerUsed;
                        } else {
                            mAttemps[result] = (powerUsed - mAttemps[result]) / 2 + mAttemps[result];
                        }
                        save();
                    }
                    break;
                case kOverFar:
                case kOverNear:
                    if (mAttemps[result] > powerUsed) {
                        mAttemps[result] = powerUsed;
                        save();
                    }
                    if (mAttemps[kScore] >= powerUsed) {
                        mAttemps[kScore] = 0;
                        save();
                    }

                case kUnderFar:
                case kUnderNear:
                    if (mAttemps[result] < powerUsed) {
                        mAttemps[result] = powerUsed;
                        save();
                    }
                    if (mAttemps[kScore] <= powerUsed) {
                        mAttemps[kScore] = 0;
                        save();
                    }
            }
        }

        public boolean equals(Object obj) {
            power myObj = (power) obj;
            if (this.distanceCM == myObj.distanceCM) {
                return true;
            } else {
                return false;
            }
        }

        private power(double distance) {
            distanceCM = distance;
            mAttemps = new double[5];
            tableRef = new NetworkTable();
            targetTable.putSubTable("" + distance, tableRef);
        }

        private double getPower() {
            if (mAttemps[kScore] != 0) {
                return mAttemps[kScore];
            }
            if (mAttemps[kOverNear] != 0) {
                if (mAttemps[kUnderNear] != 0) {
                    return (mAttemps[kOverNear] + mAttemps[kUnderNear]) / 2;
                }
                if (mAttemps[kUnderFar] != 0) {
                    return ((mAttemps[kOverNear] - mAttemps[kUnderFar]) / 4) + mAttemps[kUnderFar];
                }
                return mAttemps[kOverNear] * 0.9;
            }
            if (mAttemps[kOverFar] != 0) {
                if (mAttemps[kUnderFar] != 0) {
                    return (mAttemps[kOverFar] + mAttemps[kUnderFar]) / 2;
                }
                if (mAttemps[kUnderNear] != 0) {
                    return ((mAttemps[kOverFar] - mAttemps[kUnderNear]) / 4) + mAttemps[kUnderNear];
                }
                return mAttemps[kOverFar] * 0.75;
            }
            if (mAttemps[kUnderNear] != 0) {
                return mAttemps[kUnderNear] * 1.1;
            }
            if (mAttemps[kUnderFar] != 0) {
                return mAttemps[kUnderFar] * 1.25;
            }
            return 0;
        }

        private void save() {
            tableRef.putDouble("OverFar", mAttemps[kOverFar]);
            tableRef.putDouble("OverNear", mAttemps[kOverNear]);
            tableRef.putDouble("Score", mAttemps[kScore]);
            tableRef.putDouble("UnderFar", mAttemps[kUnderFar]);
            tableRef.putDouble("UnderNear", mAttemps[kUnderNear]);
        }
    }

    private class PowerVector extends Vector {

        private PowerVector(NetworkTable subTable) {
            init(subTable);
        }

        private PowerVector() {
            NetworkTable subTable = new NetworkTable();
            targetTable.putSubTable("power", subTable);
            init(subTable);
        }

        public boolean contains(double elem) {
            power tmp = (power) this.firstElement();
            for (int i = 0; i < this.size(); i++) {
                if (elem > tmp.distanceCM) {
                    continue;
                }
                if (elem < tmp.distanceCM) {
                    return false;
                }
                return true;
            }
            return false;
        }

        public int indexOf(double elem) {
            power tmp = (power) this.firstElement();
            for (int i = 0; i < this.size(); i++) {
                if (elem > tmp.distanceCM) {
                    continue;
                }
                if (elem < tmp.distanceCM) {
                    return -1;
                }
                return i;
            }
            return -1;
        }

        private void init(NetworkTable subTable) {
//            NetworkListener listener = new NetworkListener() {
//
//                public void valueChanged(String key, Object value) {
//                    System.out.println("Got a change" + key);
//                }
//
//                public void valueConfirmed(String key, Object value) {
//                    System.out.println("Got a confirm" + key);
//                }
//            };
//            subTable.addListenerToAll(listener);
            Vector keys = subTable.getKeys();
            for (int i = 0; i < keys.size(); i++) {
                NetworkTable tmp;
                try {
                    tmp = subTable.getSubTable((String) keys.elementAt(i));
                    power p = new power(Double.parseDouble(keys.elementAt(i).toString()), tmp.getDouble("OverFar"), tmp.getDouble("OverNear"),
                            tmp.getDouble("Score"), tmp.getDouble("UnderFar"), tmp.getDouble("UnderNear"), tmp);
                } catch (NetworkTableKeyNotDefined ex) {
                    ex.printStackTrace();
                }
            }
        }
    };

    /*
     * private class PowerTable extends NetworkTable { Vector myElements;
     *
     * public PowerTable() { myElements=new Vector(); myElements. } public
     * boolean contains(double elem) { return
     * this.containsKey(java.lang.Double.toString(elem)); }
     *
     * public power get(double elem) { try { NetworkTable tmp=(NetworkTable)
     * super.getValue(java.lang.Double.toString(elem)); power rc=new power(elem)
     * } catch (NetworkTableKeyNotDefined ex) { return null; } }
     *
     * private void add(power thisShot) { super.p }
     *
     * };
     */
    void addShot(double distance, double powerUsed, double angle, int result) {
        power thisShot;
        System.out.println("shot size=" + mPowerTrys.size());
        if (mPowerTrys.size() == 0) {
            thisShot = new power(distance);
            thisShot.addAttemp(powerUsed, angle, result);
            for (int i = 0; i < mPowerTrys.size(); i++) {
                if (distance > ((power) mPowerTrys.elementAt(i)).distanceCM) {
                    System.out.println("new shot size=" + distance);
                    mPowerTrys.insertElementAt(thisShot, i);
                    return;
                }
            }
            mPowerTrys.addElement(thisShot);
        } else {
            int idx = mPowerTrys.indexOf(distance);
            if (idx >= 0) {
                System.out.println("replace shot=" + distance);
                thisShot = (power) mPowerTrys.elementAt(idx);
                thisShot.addAttemp(powerUsed, angle, result);
            } else {
                thisShot = new power(distance);
                System.out.println("new shot=" + distance);
                thisShot.addAttemp(powerUsed, angle, result);
                for (int i = 0; i < mPowerTrys.size(); i++) {
                    if (distance > ((power) mPowerTrys.elementAt(i)).distanceCM) {
                        mPowerTrys.insertElementAt(thisShot, i);
                        return;
                    }
                }
                mPowerTrys.addElement(thisShot);
            }
        }
    }

    double getPower(double distance) {
        power thisShot;
        double returnValue;
        int idx = mPowerTrys.indexOf(distance);
        if (idx >= 0) {
            thisShot = (power) mPowerTrys.elementAt(idx);
            if (thisShot != null) {
                returnValue = thisShot.getPower();
                return returnValue;
            }
        }
        for (int i = 0; i < mPowerTrys.size(); i++) {
            power tmp = (power) mPowerTrys.elementAt(i);
            if (distance > tmp.distanceCM) {
                if (i == 0) {
                    returnValue = distance / tmp.distanceCM * tmp.getPower();
                } else {
                    power before = (power) mPowerTrys.elementAt(i - 1);
                    double y = tmp.distanceCM;
                    double x = before.distanceCM;
                    double b = tmp.getPower();
                    double a = before.getPower();
                    double z = distance;
                    double c = (((b - a) * (z - x)) / (y - x)) + a;
                    returnValue = c;
                    return returnValue;
                }
            }
        }
        power tmp = (power) mPowerTrys.lastElement();
        double y = tmp.distanceCM;
        double x = 0;
        double b = tmp.getPower();
        double a = 0;
        double z = distance;
        double c = (((b - a) * (z - x)) / (y - x)) + a;
        returnValue = c;
        return returnValue;
    }
}
